from Class.Camera import Camera
from Class.Calcuation import *
import cv2
import copy

from Class.FrankaPanda import FrankaPanda
from Class.Message import HOME_JOINT_7

class Mouse():

    def __init__(self,depth_map):
        self.x = None
        self.y = None
        self.depth_map = depth_map
        self.down = False

    def mouse_callback(self,event,x,y,flags,param):
        
        if event == cv2.EVENT_LBUTTONDOWN:
            self.x = x
            self.y = y
            self.down = True

    def getpoint(self):
        self.down = False
        return self.x,self.y 

rospy.init_node('tf_subscriber') 

franka = FrankaPanda()
franka.Go(HOME_JOINT_7)
cv2.waitKey(6000)

Pt = PointTransform()

cam = Camera()
img = cam.getimage()
depth_img = cam.getdepthimage()

mouse = Mouse(depth_img)
cv2.imshow("1",img)
cv2.setMouseCallback("1",mouse.mouse_callback)



while 1:
    key = cv2.waitKey(1)


    if mouse.down == True:

        x,y = mouse.getpoint()

        dis =     depth_img[y,x]
        print(dis)
        if dis == 0:
            print("Depth get distance bad.")
            continue

        point3d = np.array(Point2DTo3D([x,y],dis))

        try:
            res = Pt.transformation(point3d/1000)
            ores = Pt.transformation(point3d/1000)
        except:
            print("Calcution error!")
            continue
        
        res[2] = res[2] +0.1
        resJoint = franka.MoveTo(res)
        print(resJoint)
        franka.Go(resJoint)

    if key == ord("a"):
        franka.GraspClose()

    if key == ord("d"):
        franka.GraspOpen()
    
    if key == ord("s"):
        res[2] = res[2] - 0.03
        resJoint = franka.MoveTo(res)
        print(resJoint)
        franka.Go(resJoint)

    if key == ord("w"):
        res[2] = res[2] + 0.05
        resJoint = franka.MoveTo(res)
        print(resJoint)
        franka.Go(resJoint)

    if key == ord("e"):
        res[2] = res[2] - 0.16
        resJoint = franka.MoveTo(res)
        print(resJoint)
        franka.Go(resJoint)

        cv2.waitKey(2000)

        franka.GraspClose()

        cv2.waitKey(3000)

        
        resJoint = franka.MoveTo(ores)
        print(resJoint)
        franka.Go(resJoint)
        
       